#include "geometry_msgs/msg/transform.hpp"
#include "rclcpp/rclcpp.hpp"
#include "robot_msgs/msg/robot_status.hpp"
#include "robot_msgs/srv/robot_move.hpp"

class DummyRobot_Controller : public rclcpp::Node
{
  private:
	geometry_msgs::msg::Pose position;
	unsigned int status;
	rclcpp::Client<robot_msgs::srv::RobotMove>::SharedPtr client;
	double time = 0;
	rclcpp::TimerBase::SharedPtr timer;

	// to send a movement command to the robot node
	void command(double x, double y, double z, int c)
	{
		auto req = std::make_shared<robot_msgs::srv::RobotMove_Request>();
		req->control = c;
		req->transform.translation.x = x;
		req->transform.translation.y = y;
		req->transform.translation.z = z;
		client->async_send_request(
			req, std::bind(&DummyRobot_Controller::req_callback, this, std::placeholders::_1));
	}

	// called every time a request is finished with a response
	void req_callback(rclcpp::Client<robot_msgs::srv::RobotMove>::SharedFuture res)
	{
		auto resp = res.get();
		// store
		position = resp->position;
		status = resp->status;
		RCLCPP_INFO(this->get_logger(), "robot: x(%f), y(%f), z(%F), s(%d)",
					position.position.x, position.position.y, position.position.z, status);
	}

	// called every time the timer expires
	void timer_callback()
	{
		time += 0.1;
		this->command(sin(time), cos(time), tan(time), 1);
	}

  public:
	DummyRobot_Controller(std::string name) : Node(name)
	{
		int wait = 1;
		RCLCPP_INFO(this->get_logger(), "Robot controller created with name %s", name.c_str());
		client = this->create_client<robot_msgs::srv::RobotMove>("RobotMove");
		while (!client->wait_for_service(std::chrono::milliseconds(100)))
		{
			if (wait)
			{
				RCLCPP_INFO(this->get_logger(), "Waiting for service, please run robot node");
				wait = 0;
			}
		}
		this->command(0, 0, 0, 1);
		timer = this->create_wall_timer(std::chrono::milliseconds(500),
								std::bind(&DummyRobot_Controller::timer_callback, this));
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<DummyRobot_Controller>("robot_controller");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
